#include "chassis_power_control.h"
#include "referee.h"
#include "arm_math.h"
#include "detect_task.h"
#include "remote_control.h"
#include "pid.h"
#include "voltage_task.h"
#include "chassis_task.h"

extern sup_measure_t sup_cap[1]; // 电容数据
extern RC_ctrl_t rc_ctrl;
uint8_t cap_state = 0;//超级电容默认关闭，调试时可以强制打开设置为1
static const fp32 buffer_pid[3] = {2.0f, 0.0f, 0.0f}; //参数待定
static int power_control_time = 0;

void chassis_power_control(chassis_move_t *chassis_power_control)
{
    PID_init(&chassis_power_control->buffer_pid, PID_POSITION, buffer_pid, 130, 50); // 缓冲PID初始化
    // supcap_measure = get_supercap_measure(); // 获取超级电容数据
	uint16_t max_power_limit = 0;
	fp32 chassis_max_power = 0;//这里的max_power实际上是后续底盘电机可用功率
    float input_power = 0;		 // 从电池（裁判系统）输入的功率
	float initial_give_power[4]; // PID计算的初始功率
	float initial_total_power = 0;
	fp32 scaled_give_power[4];

	fp32 chassis_power = 0.0f;
	uint16_t chassis_power_buffer = 0.0f;

	fp32 toque_coefficient = 1.99688994e-6f; // (20/16384)*(0.3)*(187/3591)/9.55
	fp32 k1 = 1.23e-07;						 // k1
	fp32 k2 = 1.453e-07;					 // k2
	fp32 constant = 4.081f;

	get_chassis_power_and_buffer(&chassis_power, &chassis_power_buffer);
	PID_calc(&chassis_power_control->buffer_pid, chassis_power_buffer, 30);//30是我希望留存的缓冲能量（？
	get_chassis_power_limit(&max_power_limit);

	input_power = max_power_limit - chassis_power_control->buffer_pid.out; // 输入功率浮动在最大功率下（目前不太明白

	power_control_time++;
	if (power_control_time == 10)
	{
   		CAN_cmd_supercap(input_power); // 设置超级电容的输入功率
		power_control_time = 0;
	}
	
	if (rc_ctrl.key.v & KEY_PRESSED_OFFSET_SHIFT)//按下shift使用超级电容放电
	{
		cap_state = 1;
	}
    else
    {
        cap_state = 0;
    }
	if (sup_cap[0].capacitance_percentage > 5)//超级电容剩余容量大于5％
	{
		if (cap_state == 0)
		{
			chassis_max_power = input_power + 5; // 不用超级电容，底盘使用功率略大于最大功率，避免电容器一直充满，提高能源利用率
		}
		else
		{
			chassis_max_power = input_power + 100;//使用超级电容，底盘功率狠狠用！
		}
	}
	else
	{
		chassis_max_power = input_power;
	}

	for (uint8_t i = 0; i < 4; i++) // 首先获取所有初始电机功率和总电机功率
	{
		initial_give_power[i] = chassis_power_control->motor_speed_pid[i].out * toque_coefficient * chassis_power_control->motor_chassis[i].chassis_motor_measure->speed_rpm +
								k1 * chassis_power_control->motor_chassis[i].chassis_motor_measure->speed_rpm * chassis_power_control->motor_chassis[i].chassis_motor_measure->speed_rpm +
								k2 * chassis_power_control->motor_speed_pid[i].out * chassis_power_control->motor_speed_pid[i].out + constant;

		if (initial_give_power < 0) // 不包括负功率（暂时性）
			continue;
		initial_total_power += initial_give_power[i];
	}

	if (initial_total_power > chassis_max_power) // 确定是否大于最大功率
	{
		fp32 power_scale = chassis_max_power / initial_total_power;
		for (uint8_t i = 0; i < 4; i++)
		{
			scaled_give_power[i] = initial_give_power[i] * power_scale; // 获得缩放功率
			if (scaled_give_power[i] < 0)
			{
				continue;
			}

			fp32 b = toque_coefficient * chassis_power_control->motor_chassis[i].chassis_motor_measure->speed_rpm;
			fp32 c = k2 * chassis_power_control->motor_chassis[i].chassis_motor_measure->speed_rpm * chassis_power_control->motor_chassis[i].chassis_motor_measure->speed_rpm - scaled_give_power[i] + constant;

			if (chassis_power_control->motor_speed_pid[i].out > 0) // 根据原始力矩的方向选择计算公式
			{
				fp32 temp = (-b + sqrt(b * b - 4 * k1 * c)) / (2 * k1);
				if (temp > 16000)
				{
					chassis_power_control->motor_speed_pid[i].out = 16000;
				}
				else
					chassis_power_control->motor_speed_pid[i].out = temp;
			}
			else
			{
				fp32 temp = (-b - sqrt(b * b - 4 * k1 * c)) / (2 * k1);
				if (temp < -16000)
				{
					chassis_power_control->motor_speed_pid[i].out = -16000;
				}
				else
					chassis_power_control->motor_speed_pid[i].out = temp;
			}
		}
	}
}
